#include <iostream>
#include <algorithm>  // std::reverse()
#include <string>
#include <cmath>
#include <time.h>
#include <stdlib.h>

// ROS include
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Point.h>

#include <Eigen/Dense>
#include <Eigen/SVD>

#define DEB(X) \
  std::cout<<#X<<": "<<X<<std::endl

/** TargetDetector detects a target in a laser scan. 
 * The target corresponds to the nearest group of scan points in
 * the foreground. 
 * It subscribes to a laser scan topic (set correct "scan_topic" param)
 * and publishes a point topic representing the position of the target
 * (set the correct "target_pos_topic" param). 
 */
class TargetDetector
{
  typedef struct{
    geometry_msgs::Point p;
    double r;
    int ttl;
  }trackedObj;
  std::vector<trackedObj> track_;
public:
  typedef std::pair<int,int> interval_type;
  typedef std::vector<interval_type> interval_vector;

  enum Index {X = 0, Y = 1, R = 2};

  ros::NodeHandle nh_;
  ros::Subscriber scan_sub_;
  sensor_msgs::LaserScan scan_;
  ros::Publisher target_pos_pub_;
  geometry_msgs::Point target_pos_;

  double range_thr_;          // threshold to split scan into segments
  double range_min_;          // minimum value of a valid range
  double range_max_;          // maximum value of a valid range
  double angle_min_;          // first angle of the considered FoV
  double angle_max_;          // last angle of the considered FoV
  double target_radius_;      // approximate radius of circular target
  double target_radius_toll_; // relative tollerance on target radius
  bool flipped_scan_;         // check if the scan has to be flipped
  
  bool limited_;
  
  int minData;
  int minForModel;
  int numIteration;
  double modelTh;

  TargetDetector(std::string configfile = "") : nh_ ("~")
  {
    srand(time(0));
    // Reads configuration params
    std::string scan_topic;
    std::string target_pos_topic;
    nh_.param<std::string>("scan_topic", scan_topic, "/scan");
    nh_.param<std::string>("target_pos_topic", target_pos_topic, "/target_pos");
    nh_.param<double>("range_thr", range_thr_, 0.8);
    nh_.param<double>("range_min", range_min_, 0.05);
    nh_.param<double>("range_max", range_max_, 8.00);
    nh_.param<double>("angle_min", angle_min_, -M_PI_2);
    nh_.param<double>("angle_max", angle_max_,  M_PI_2);
    nh_.param<double>("target_radius", target_radius_, 0.37);
    nh_.param<double>("target_radius_toll", target_radius_toll_, 0.1);
    nh_.param<int>("min_data_consensus", minData, 3);
    nh_.param<int>("min_for_model", minForModel, 10);
    nh_.param<int>("num_iteration", numIteration, 10);
    nh_.param<double>("modelTh", modelTh, 0.05);
    nh_.param<bool>("flipped",flipped_scan_,false);
    nh_.param<bool>("limited_search",limited_,false);

    // Initializes subscribers and publishers
    ROS_INFO("Subscribing to topic %s",scan_topic.c_str()); 
    scan_sub_ = nh_.subscribe (scan_topic, 1, &TargetDetector::newLaserScan, this);
    ROS_INFO("Advertising topic %s",target_pos_topic.c_str()); 
    target_pos_pub_ = nh_.advertise<geometry_msgs::Point>(target_pos_topic, 1);
  }

  ~TargetDetector() { }
  
  bool checkDistance(double x1, double y1,double x2, double y2, double th){
    return std::sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2))<=th;
  }
  
  void newLaserScan(const sensor_msgs::LaserScan& scan_msg) 
  {
    scan_ = scan_msg;
    if (!scan_.ranges.empty() && flipped_scan_) {
      ROS_INFO("Flipping scan");
      std::reverse(scan_.ranges.begin(),scan_.ranges.end());
      std::reverse(scan_.intensities.begin(),scan_.intensities.end());
    }
   
    // Searches all candidate range intervals 
    ROS_INFO("Searching intervals");
    interval_vector intervals;
    splitScan(intervals); 
    ROS_INFO("\nNew scan: found %d intervals",intervals.size());

    // Visits all intervals 
    geometry_msgs::Point center_best,fakeCenter;
    double r_best=1000000,bestError=1;
    bool find=false;
    interval_vector::iterator beg = intervals.begin();
    interval_vector::iterator end = intervals.end();
    fakeCenter.x=NAN;
    fakeCenter.y=NAN;
    fakeCenter.z=NAN;
    //limito la ricerca della palla togliendo il primo e l'ultimo intevallo
    if(limited_){
      beg++;
      end--;
    }
    
    for (beg; beg != end; ++beg) {
      geometry_msgs::Point center;
      double r,error;
      //estraggo con ransac il modello e l'errore calcolato come (size of data in consensus set/size of data)
      error=extractCircleRansac(*beg,center,r);
      ROS_INFO("interval %d,%d: center (%f,%f), radius %f, error %f",
               beg->first,beg->second,center.x,center.y,r,error);
      if (::fabs(r - target_radius_) < target_radius_toll_ * target_radius_){ //se il modello estratto ha un raggio accettabile valuto il suo errore
        if (error<bestError) {                                                //con il bestError attuale
          center_best = center;
          r_best = r;
          bestError=error;
        }
        find=true;
      }
    }
    
    if(find){
      if(track_.size()==0){
        trackedObj t;
        t.r=r_best;
        t.p=center_best;
        t.ttl=0;
        track_.push_back(t);
      } else {
        bool different=true;
        for(std::vector<trackedObj>::iterator i=track_.begin();i!=track_.end();++i){
          if(checkDistance(i->p.x,i->p.y,center_best.x,center_best.y,0.10)){
            i->p.x=center_best.x;
            i->p.y=center_best.y;
            i->r=r_best;
            ++(i->ttl);
            different=false;
            if(i->ttl>6){
              ROS_INFO("Best target\n best center (%f,%f), radius %f",center_best.x,center_best.y,r_best);
              center_best.z=r_best;
              target_pos_pub_.publish(center_best);
              //--(i->ttl);
            }
          } else {
            if(i->ttl==1){
              track_.erase(i);
              --i;
            } else
              --(i->ttl);
          }
        }
        if(different){
          trackedObj t;
          t.r=r_best;
          t.p=center_best;
          t.ttl=0;
          track_.push_back(t);
        }
      }
    } else {
      ROS_INFO("No target found\n  target radius %f, candidate radius %f, max difference %f",
      target_radius_,r_best,target_radius_toll_ * target_radius_);
      target_pos_pub_.publish(fakeCenter);
    }
      
    
    /*
    if (find) {
      ROS_INFO("Best target\n best center (%f,%f), radius %f",center_best.x,center_best.y,r_best);
			center_best.z=r_best;
      target_pos_pub_.publish(center_best);
    }
    else {
      ROS_INFO("No target found\n  target radius %f, candidate radius %f, max difference %f",
        target_radius_,r_best,target_radius_toll_ * target_radius_);
      target_pos_pub_.publish(fakeCenter);
    }
    */
    //std::getchar();
  }

protected:
  /** Splits the scan into intervals where range discontinuities occur.
   */
  void splitScan(interval_vector& intervals) 
  {
    if (scan_.ranges.empty()) return;
    unsigned int imin = (unsigned int)((angle_min_ - scan_.angle_min) / scan_.angle_increment);
    unsigned int imax = (unsigned int)((angle_max_ - scan_.angle_min) / scan_.angle_increment);  
    double rangePrev = 0.0;
    interval_type tmp;
    tmp.first = 0;
    if (imin < scan_.ranges.size()) {
      rangePrev = scan_.ranges.at(imin);
      tmp.first = imin;
    }
    ROS_INFO("  scanning ranges from %d to %d",imin,imax);
    for (unsigned int i = imin; i < scan_.ranges.size() && i < imax; ++i) {
      if (range_min_ <= scan_.ranges.at(i) && scan_.ranges.at(i) <= range_max_) {    
        if (::fabs(scan_.ranges.at(i) - rangePrev) > range_thr_) {
          tmp.second = i;
          intervals.push_back(tmp);
          std::cout << "range discontinuity at " << i << ": " << scan_.ranges.at(i) 
                    << " -" << rangePrev << std::endl;
          tmp.first = i;
        }
        rangePrev = scan_.ranges.at(i);
      }
    }
    tmp.second = scan_.ranges.size();
    intervals.push_back(tmp);
  }
  /*
  void extractCircle2(const interval_type& ip,geometry_msgs::Point& center,double& r) 
  {
    unsigned int num = ip.second-ip.first;
    Eigen::MatrixXd A(num,3);
    Eigen::VectorXd b(num);
    Eigen::Vector3d res;
    center.x = center.y = 0.0;
    for (unsigned int i = ip.first; i < scan_.ranges.size() && i < ip.second; ++i) {
      double t = scan_.angle_min + i * scan_.angle_increment;
      double x = scan_.ranges.at(i) * ::cos(t);
      double y = scan_.ranges.at(i) * ::sin(t);
      unsigned int j = i - ip.first;
      A(j,0) = x;
      A(j,1) = y;
      A(j,2) = 1.0;
      b(j) = x*x + y*y;
      center.x += x;
      center.y += y;
    }
    res = (A.transpose() * A).inverse() * A.transpose() * b;
//    std::cout << "\ninterval " << ip.first << "," << ip.second << std::endl;
//    std::cout << "mean " << center.x/num << "," << center.y/num << std::endl;
//    std::cout << "res:\n" << res << std::endl;
    center.x = 0.5 * res(X);
    center.y = 0.5 * res(Y);
    center.z = 0.0;
    r = ::sqrt(res(R) + center.x*center.x + center.y*center.y);
//    std::cout << "center " << center.x << "," << center.y << std::endl;
  }
  
  void extractCircle3(const interval_type& ip,geometry_msgs::Point& center,double& r) 
  {
    unsigned int num = ip.second-ip.first;
    Eigen::MatrixXd A(num,3);
    Eigen::VectorXd b(num);
    Eigen::Vector3d res;
    center.x = center.y = 0.0;
    for (unsigned int i = ip.first; i < scan_.ranges.size() && i < ip.second; ++i) {
      double t = scan_.angle_min + i * scan_.angle_increment;
      double x = scan_.ranges.at(i) * ::cos(t);
      double y = scan_.ranges.at(i) * ::sin(t);
      unsigned int j = i - ip.first;
      A(j,0) = x;
      A(j,1) = y;
      A(j,2) = 1.0;
      b(j) = x*x + y*y;
      center.x += x;
      center.y += y;
    }
    //Eigen::FullPivHouseholderQR<Eigen::MatrixXd> dec(A);
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A,Eigen::ComputeThinU | Eigen::ComputeThinV);
    //std::cout<<"A: "<<A<<std::endl;
    //res = A.jacobiSvd(Eigen::ComputeThinU || Eigen::ComputeThinV).solve(b);
    res= svd.solve(b);//dec.solve(b);
//    std::cout << "\ninterval " << ip.first << "," << ip.second << std::endl;
//    std::cout << "mean " << center.x/num << "," << center.y/num << std::endl;
//    std::cout << "res:\n" << res << std::endl;
    center.x = 0.5 * res(X);
    center.y = 0.5 * res(Y);
    center.z = 0.0;
    r = ::sqrt(res(R) + center.x*center.x + center.y*center.y);
//    std::cout << "center " << center.x << "," << center.y << std::endl;
  }
  */
  void fillCons(std::vector<Eigen::Vector3d>& data, std::vector<bool>& inElab, std::vector<Eigen::Vector3d>& cons,int minData){
    cons.clear();
    int i;
    for(int j=0;j<minData;++j){
      //inserisco minData element scelti a caso in cons
      
      //finchè inElab restituisce true continuo a cercare un elemento in data scegliendolo a caso
      while(inElab[ i=rand()%data.size() ]);
      //una volta trovato l'elemento setto il flag di inElab e lo inserisco in cons
      inElab[i]=true;
      cons.push_back(data[i]);
    }
  }
  void generateLine2(const std::vector<Eigen::Vector3d >& cons, Eigen::Vector3d& model){
    // Least square line. We pick the minimum (rho,theta) that minimizes error:
    // \sum_i (x_i * cos(theta) + y_i * sin(theta) - rho)^2
    double sxx = 0.0;
    double syy = 0.0;
    double sxy = 0.0;
    double sx = 0.0;
    double sy = 0.0;
    int num = 0;
    for (unsigned int i =0; i < cons.size(); ++i) {
      sxx += cons[i](0) * cons[i](0);
      syy += cons[i](1) * cons[i](1);
      sxy += cons[i](0) * cons[i](1);
      sx += cons[i](0);
      sy += cons[i](1);
      ++num;
    }

    double msxx = (sxx - sx * sx / num) / num;
    double msyy = (syy - sy * sy / num) / num;
    double msxy = (sxy - sx * sy / num) / num;
    double b = 2.0 * msxy;
    double a = msxx;
    double c = msyy;
    double theta = 0.5 * (atan2(b, a - c) + M_PI);
    theta = atan2(sin(theta), cos(theta));
    double rho = (sx * cos(theta) + sy * sin(theta)) / num;
    if (rho < 0) {
      theta = atan2(sin(theta + M_PI), cos(theta + M_PI));
      rho = -rho;
    }
    model(0) = cos(theta);
    model(1) = sin(theta);
    model(2) = -rho;
  }
  void generateCircle(const std::vector<Eigen::Vector3d >& cons, Eigen::Vector3d& model) 
  {
    //calcolo il modello a partire dai dati di cons
    //il modello rappresenta un vettore fatto così model=[center.x ; center.y ; r]
    Eigen::MatrixXd A(cons.size(),3);
    Eigen::VectorXd b(cons.size());
    Eigen::Vector3d res;
    for (unsigned int i = 0; i < cons.size(); ++i) {
      A(i,0) = cons[i](0);
      A(i,1) = cons[i](1);
      A(i,2) = 1.0;
      b(i) = cons[i](0)*cons[i](0) + cons[i](1)*cons[i](1);
    }
    //Eigen::FullPivHouseholderQR<Eigen::MatrixXd> qr(A);
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A,Eigen::ComputeThinU | Eigen::ComputeThinV);
    //std::cout<<"A: "<<A<<std::endl;
    //res = (A.transpose() * A).inverse() * A.transpose() * b;
    res= svd.solve(b);
    //res= qr.solve(b);
    model(0) = 0.5 * res(0);
    model(1) = 0.5 * res(1);
    model(2) = std::sqrt(res(2) + model(0)*model(0) + model(1)*model(1));
  }
  bool checkLine(Eigen::Vector3d& point,Eigen::Vector3d& model,double& threshold){
    return (std::fabs(model(0)*point(0)+model(1)*point(1)+model(2))/sqrt(model(0)*model(0)+model(1)*model(1)))<threshold;
  }
  bool checkCircle(Eigen::Vector3d& point,Eigen::Vector3d& model,double& threshold){
    //controllo che point sia ad una distanza dalla circonferenza rappresentata da model, inferiore a threshold
    return std::fabs(std::sqrt((point(0)-model(0))*(point(0)-model(0))+(point(1)-model(1))*(point(1)-model(1)))-model(2))<threshold;
  }
  
  std::pair<double,double> Ransac(std::vector<Eigen::Vector3d>& data,int minData, int minForModel, int numIteration, std::vector<Eigen::Vector3d>& bestConsensus, Eigen::Vector3d& bestModel, double modelTh){
    //inizializzo il modello migliore
    Eigen::Vector3d bestLineModel(0,0,0);
    std::vector<Eigen::Vector3d> bestConsensusLine;
    bestConsensus.clear();
    bestModel=Eigen::Vector3d(0,0,0);
    std::pair<double,double> error(0.0,0.0);
    for(int iter=0;iter<numIteration;++iter){
      //per ogni iterazione inizializzo il vettore dei flag inElab e riempio il vettore consensus con dati
      //scelti a caso da data,
      std::vector<bool> inElab(data.size(),false);
      std::vector<Eigen::Vector3d> consensusCircle,consensusLine;
      fillCons(data,inElab,consensusCircle,minData);
      consensusLine.push_back(consensusCircle[0]);
      consensusLine.push_back(consensusCircle[1]);
      //da consensus calcolo possibleModel
      Eigen::Vector3d possibleModelCircle(0,0,0),possibleModelLine(0,0,0);
      generateCircle(consensusCircle,possibleModelCircle);
      generateLine2(consensusLine,possibleModelLine);
      
      //calcolo il consensus set valutando la distanza di ogni punto non già presente nel vettore
      //con il possibleModel, se questa distanza p inferiore a modelTh allora il punto è un inlier
      for(int i=0;i<data.size();++i)
        if(!inElab[i]){
          if(checkCircle(data[i],possibleModelCircle,modelTh)){
            consensusCircle.push_back(data[i]);
            inElab[i]=true;
          }
          if(checkLine(data[i],possibleModelLine,modelTh)){
            consensusLine.push_back(data[i]);
            inElab[i]=true;
          }
        }
      //valuto che il nuovo consensus set abbia abbastanza consensi
      if(consensusCircle.size()>minForModel && consensusCircle.size()>bestConsensus.size()){
        //ricalcolo il nuovo modello basandomi sull'intero consensus set
        generateCircle(consensusCircle,bestModel);
        bestConsensus=consensusCircle;
      }
      if(consensusLine.size()>minForModel && consensusLine.size()>bestConsensusLine.size()){
        //ricalcolo il nuovo modello basandomi sull'intero consensus set
        generateLine2(consensusLine,bestLineModel);
        bestConsensusLine=consensusLine;
      }
    }
    //ritorno l'errore del consensus set calcolandolo come la percentuale di outlier su tutti i dati
    std::cout<<"consensus circle: "<<bestConsensus.size()<<"  data: "<<data.size()<<std::endl;
    std::cout<<"consensus circle %: "<<double(bestConsensus.size()*100)/data.size()<<std::endl;
    std::cout<<"consensus line: "<<bestConsensusLine.size()<<"  data: "<<data.size()<<std::endl;
    std::cout<<"consensus line %: "<<double(bestConsensusLine.size()*100)/data.size()<<std::endl;
    error.second=1-(double(bestConsensus.size())/data.size());
    error.first=1-(double(bestConsensusLine.size())/data.size());
    return error;
  }

  double extractCircleRansac(const interval_type& ip,geometry_msgs::Point& center,double& r) {
    std::vector<Eigen::Vector3d> bestConsensus;
    Eigen::Vector3d bestModel(0,0,0);
    unsigned int num = ip.second-ip.first;
    std::pair<double,double> error(1,1);
    //se l'intervallo non ha abbastanza dati non lo prendo in considerazione
    if(num>minData){
      std::vector<Eigen::Vector3d> data;
      for (unsigned int i = ip.first; i < scan_.ranges.size() && i < ip.second; ++i) {
        double t = scan_.angle_min + i * scan_.angle_increment;
        double x = scan_.ranges.at(i) * ::cos(t);
        double y = scan_.ranges.at(i) * ::sin(t);
        data.push_back(Eigen::Vector3d(x,y,0));
      }
      //dopo aver trasformato i dati dell'intervallo di scansione calcolo il modello con ransac
      error=Ransac(data,minData,minForModel,numIteration,bestConsensus,bestModel,modelTh);
    }
    if(error.first>error.second){
      center.x = bestModel(0);
      center.y = bestModel(1);
      center.z = 0.0;
      r = bestModel(2);
      return error.second;
    } else {
      return 1;
    }
  }
};

int main (int argc, char** argv)
{
  ros::init (argc, argv, "target_detector");
  TargetDetector td;  
  ros::Rate loop_rate(10);
  while (ros::ok()) {
    ros::spinOnce();
    loop_rate.sleep();
  }
  return (0);
}


